package tazadum.engine.math.curve;

import java.util.Collections;
import java.util.LinkedList;
import java.util.List;

import tazadum.engine.math.Vector;
import tazadum.engine.math.Vectors;

public class BeizerCurve2d implements Curve {
	protected List<Vector> nodes = new LinkedList<Vector>();

	@Override
	public void reset() {
		nodes.clear();
	}

	@Override
	public void add(final Vector node) {
		nodes.add(node);
		Collections.sort(nodes, Vectors.SORT_BY_Z);
	}

	@Override
	public void add(final Vector... nodes) {
		for (final Vector node : nodes) {
			this.nodes.add(node);
		}
		Collections.sort(this.nodes, Vectors.SORT_BY_Z);
	}

	@Override
	public Vector calculate(final float time) {
		if (nodes.size() < 4) {
			return null;
		}
		final Vector[] points = findPoints(time);
		final float delta = points[3].z - points[0].z;
		final float alpha = (time - points[0].z) / delta;
		final float x = interpolate(alpha, points[0].x, points[1].x, points[2].x, points[3].x);
		final float y = interpolate(alpha, points[0].y, points[1].y, points[2].y, points[3].y);
		return new Vector(x, y, alpha);
	}

	private Vector[] findPoints(final float time) {
		final Vector[] list = new Vector[4];
		int index = 0;
		for (final Vector v : nodes) {
			if (time < v.z && index >= 4) {
				break;
			}
			list[0] = list[1];
			list[1] = list[2];
			list[2] = list[3];
			list[3] = v;
			index++;
		}
		return list;
	}

	protected final float interpolate(final float a, final float A, final float B, final float C, final float D) {
		return a * a * a * A + 3 * a * a * (1 - a) * B + 3 * a * (1 - a) * (1 - a) * C + (1 - a) * (1 - a) * (1 - a)
				* D;
	}
}
